//
// Created by 16933 on 2022/5/1.
//

#include "bsp_MPU6050.h"
#include "bsp_I2C_For_MPU.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include <math.h>
short dmp_gyro_data[3];
/*! @brief 写一个字节数据
 * @param reg：写入的寄存器地址
 * @param data：写入的数据
 * @return 1：写入失败 0：写入成功
 * */
uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data)
{
    I2C_Start_For_MPU();
    I2C_Send_Byte_For_MPU(MPU_WRITE);
    if (I2C_Wait_ACK_For_MPU())
    {
        I2C_Stop_For_MPU();
        return 1;
    }
    I2C_Send_Byte_For_MPU(reg);
    I2C_Wait_ACK_For_MPU();
    I2C_Send_Byte_For_MPU(data);
    if (I2C_Wait_ACK_For_MPU())
    {
        I2C_Stop_For_MPU();
        return 1;
    }
    I2C_Stop_For_MPU();
    return 0;
}


/*!@brief  读一个字节 数据
 * @param reg：要读的寄存器地址
 * @return 读出的数据
 * */
uint8_t MPU_Read_Byte(uint8_t reg)
{
    uint8_t res;
    I2C_Start_For_MPU();
    I2C_Send_Byte_For_MPU((MPU6050_ADDRESS<<1)|0);       //发送器件地址+写命令
    I2C_Wait_ACK_For_MPU();		                              //等待应答
    I2C_Send_Byte_For_MPU(reg);	                      //写寄存器地址
    I2C_Wait_ACK_For_MPU();		                              //等待应答
    I2C_Start_For_MPU();
    I2C_Send_Byte_For_MPU((MPU6050_ADDRESS<<1)|1);      //发送器件地址+读命令
    I2C_Wait_ACK_For_MPU();		                             //等待应答
    res=I2C_Receive_Byte_For_MPU(0);                          //读取数据,发送nACK
    I2C_Stop_For_MPU();			                             //产生一个停止条件
    return res;
}


/*! @brief 连续写多个字节
 * @param addr：外设地址
 * @param reg：寄存器地址
 * @param len：写入长度
 * @param buf：写入的数据
 * @return 1:失败  0：成功
 * */
uint8_t MPU_Write_Bytes(uint8_t reg,uint8_t len,uint8_t *buf)
{
    uint8_t i;
    I2C_Start_For_MPU();
    I2C_Send_Byte_For_MPU(MPU_WRITE);
    if (I2C_Wait_ACK_For_MPU())
    {
        I2C_Stop_For_MPU();
        return 1 ;
    }
    I2C_Send_Byte_For_MPU(reg);
    I2C_Wait_ACK_For_MPU();
    for (i = 0;i<len;i++) {
        I2C_Send_Byte_For_MPU(buf[i]);
        if (I2C_Wait_ACK_For_MPU())
        {
            I2C_Stop_For_MPU();
            return 1 ;
        }
    }
    I2C_Stop_For_MPU();
    return 0;

}

/*!
 * @brief 连续读取多个字节数据
 * @param addr：外设地址
 * @param reg：寄存器地址
 * @param len：写入长度
 * @param buf：数据存储地址
 * @return 1:失败  0：成功
 * */
uint8_t MPU_Read_Bytes(uint8_t reg,uint8_t len,uint8_t *buf)
{
    I2C_Start_For_MPU();
    I2C_Send_Byte_For_MPU(MPU_WRITE);
    if (I2C_Wait_ACK_For_MPU())
    {
        I2C_Stop_For_MPU();
        return 1 ;
    }
    I2C_Send_Byte_For_MPU(reg);
    I2C_Wait_ACK_For_MPU();
    I2C_Start_For_MPU();
    I2C_Send_Byte_For_MPU(MPU_READ);
    I2C_Wait_ACK_For_MPU();
    while(len)
    {
        if (len==1)
        {
            *buf = I2C_Receive_Byte_For_MPU(0);

        }
        else
        {
            *buf = I2C_Receive_Byte_For_MPU(1);
        }
        len--;
        buf++;

    }
    I2C_Stop_For_MPU();
    return 0;
}

/*!
 * @brief: MPU初始化
 * */
uint8_t MPU_Init()
{
    uint8_t res;
    MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);	//复位MPU6050
    MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);	//唤醒MPU6050
    MPU_Set_Gyro_Fsr(3);					//陀螺仪传感器,±2000dps
    MPU_Set_Accel_Fsr(2);					//加速度传感器,±8g
    MPU_Set_Rate(1000);						//设置采样率1000Hz
    MPU_Write_Byte(MPU_INT_EN_REG,0X00);	//关闭所有中断
    MPU_Write_Byte(MPU_USER_CTRL_REG,0X00);	//I2C主模式关闭
    MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);	//关闭FIFO
    MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);	//INT引脚低电平有效
    res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
    if(res==MPU_ADDR)//器件ID正确
    {
        MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);	//设置CLKSEL,PLL X轴为参考
        MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);	//加速度与陀螺仪都工作
        MPU_Set_Rate(1000);						//设置采样率为1000Hz
    }else return 1;
    return 0;
}



/*!
 * @brief 设置MPU6050的数字低通滤波器
 * @param lpf: 数字低通滤波频率(Hz)
 * @return 0,设置成功
 *         其他,设置失败
 * */
uint8_t MPU_Set_LPF(uint16_t lpf)
{
    uint8_t data=0;
    if(lpf>=188)data=1;
    else if(lpf>=98)data=2;
    else if(lpf>=42)data=3;
    else if(lpf>=20)data=4;
    else if(lpf>=10)data=5;
    else data=6;
    return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器
}


//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败

/*!
 * @brief 设置MPU6050陀螺仪传感器满量程范围
 * @param fsr:  0,±250dps;  1,±500dps;  2,±1000dps; 3,±2000dps
 * @return  0,设置成功
 *         其他,设置失败
 * */
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr)
{
    return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
}



/*!
 * @brief 设置MPU6050加速度传感器满量程范围
 * @param fsr:  0,±2g;1,±4g;2,±8g;3,±16g
 * @return  0,设置成功
 *         其他,设置失败
 * */
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr)
{
    return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
}



/*!
 * @brief 设置MPU6050的采样率(假定Fs=1KHz)
 * @param rate:4~1000(Hz)
 * @return  0,设置成功
 *         其他,设置失败
 * */
uint8_t MPU_Set_Rate(uint16_t rate)
{
    uint8_t data;
    if(rate>1000)rate=1000;
    if(rate<4)rate=4;
    data=1000/rate-1;
    data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);	//设置数字低通滤波器
    return MPU_Set_LPF(rate/2);	//自动设置LPF为采样率的一半
}


/*!
 * @brief 得到温度值
 * @return  温度值
 * */
float MPU_Get_Temperature(void)
{
    unsigned char  buf[2];
    short raw;
    float temp;
    MPU_Read_Bytes(MPU_TEMP_OUTH_REG,2,buf);
    raw=(buf[0]<<8)| buf[1];
    temp=(36.53+((double)raw)/340)*100;
//  temp = (long)((35 + (raw / 340)) * 65536L);
    return temp/100.0f;
}


/*!
 * @brief 得到陀螺仪值(原始值)
 * @param gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
 * @return  0,设置成功
 *         其他,设置失败
 * */
uint8_t MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
    uint8_t buf[6],res;
    res=MPU_Read_Bytes(MPU_GYRO_XOUTH_REG,6,buf);
    if(res==0)
    {
        *gx=((uint16_t)buf[0]<<8)|buf[1];
        *gy=((uint16_t)buf[2]<<8)|buf[3];
        *gz=((uint16_t)buf[4]<<8)|buf[5];
    }
    return res;
}



/*!
 * @brief 得到加速度值(原始值)
 * @param ax,ay,az:加速度x,y,z轴的原始读数(带符号)
 * @return  0,设置成功
 *         其他,设置失败
 * */
uint8_t MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    uint8_t buf[6],res;
    res=MPU_Read_Bytes(MPU_ACCEL_XOUTH_REG,6,buf);
    if(res==0)
    {
        *ax=((uint16_t)buf[0]<<8)|buf[1];
        *ay=((uint16_t)buf[2]<<8)|buf[3];
        *az=((uint16_t)buf[4]<<8)|buf[5];
    }
    return res;;
}





//陀螺仪方向设置
static signed char gyro_orientation[9] = { 1, 0, 0,
                                           0, 1, 0,
                                           0, 0, 1};
//MPU6050自测试
//返回值:0,正常
//    其他,失败
uint8_t run_self_test(void)
{
    int result;
    //char test_packet[4] = {0};
    long gyro[3], accel[3];
    result = mpu_run_self_test(gyro, accel);
//    printf("mpu_run_self_test_result=%d\n", result);
    if (result == 0x3)
    {
        /* Test passed. We can trust the gyro data here, so let's push it down
        * to the DMP.
        */
        float sens;
        unsigned short accel_sens;
        mpu_get_gyro_sens(&sens);
        gyro[0] = (long)(gyro[0] * sens);
        gyro[1] = (long)(gyro[1] * sens);
        gyro[2] = (long)(gyro[2] * sens);
        dmp_set_gyro_bias(gyro);
        mpu_get_accel_sens(&accel_sens);
        accel[0] *= accel_sens;
        accel[1] *= accel_sens;
        accel[2] *= accel_sens;
        dmp_set_accel_bias(accel);
        return 0;
    }else return 1;
}
//陀螺仪方向控制
unsigned short inv_orientation_matrix_to_scalar(
        const signed char *mtx)
{
    unsigned short scalar;
    /*
       XYZ  010_001_000 Identity Matrix
       XZY  001_010_000
       YXZ  010_000_001
       YZX  000_010_001
       ZXY  001_000_010
       ZYX  000_001_010
     */

    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;


    return scalar;
}
//方向转换
unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;      // error
    return b;
}
//空函数,未用到.
void mget_ms(unsigned long *time)
{

}


/**
 * @brief mpu6050，dmp初始化
 *  采样率 100hz
 *  Gyro FSR: +/- 2000DPS\n
 *  Accel FSR +/- 8G\n
 *  DLPF: 42Hz\n
 *  FIFO rate: 50Hz\n
 * @return 0：正常 其他：失败
 * */
uint8_t MPU_DMP_Init(void)
{
    uint8_t res=0;
    HAL_Delay(100);
    if(mpu_init()==0)	//初始化MPU6050
    {
        res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
        if(res)
        {
//            printf("set sensors failed \n");
            return 1;
        }
//        else printf("set sensors successful \n");
        res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO
        if(res)
        {
//            printf("set FIFO failed \n");
            return 2;
        }
//        else printf("set FIFO successful \n");
        res=mpu_set_sample_rate(DEFAULT_MPU_HZ);	//设置采样率
        if(res)
        {
//            printf("set sample rate failed \n");
            return 3;
        }
//        else printf("set sample rate successful \n");
        res=dmp_load_motion_driver_firmware();		//加载dmp固件
        if(res)
        {
//            printf("load DMP failed \n");
            return 4;
        }
//        else printf("load DMP successful \n");
        res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
        if(res)
        {
//            printf("set orientation failed \n");
            return 5;
        }
        else
        {
//            printf("set orientation successful \n");
        }
        res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|	//设置dmp功能
                               DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
                               DMP_FEATURE_GYRO_CAL);
        if(res)
        {
//            printf("set DMP failed \n");
            return 6;
        }
        else {
//            printf("set DMP successful \n");
        }
        res=dmp_set_fifo_rate(20);	//设置DMP输出速率(最大不超过200Hz)
        if(res)
        {
            return 7;
        }
        res=run_self_test();		//自检
        if(res)
        {
//            printf("self test failed \n");
            return 8;
        }
//        else printf("self test successful \n");
        res=mpu_set_dmp_state(1);	//使能DMP
        if(res)
        {
//            printf("enable DMP failed \n");
            return 9;
        }
//        else printf("enable DMP successful \n");
    }
//    printf("initialize MPU6050 successful \n");
    return 0;
}


//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1°   范围:-90.0° <---> +90.0°
//roll:横滚角  精度:0.1°   范围:-180.0°<---> +180.0°
//yaw:航向角   精度:0.1°   范围:-180.0°<---> +180.0°
//返回值:0,正常
//    其他,失败
uint8_t MPU_DMP_Get_Data(float *pitch,float *roll,float *yaw)
{
    float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
    unsigned long sensor_timestamp;
    short gyro[3], accel[3], sensors;
    unsigned char more;
    long quat[4]; //得到的四元数
    if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;
    //sensors参数相当于是一个标志位寄存器，如果读出的数据中存在某个传感器的数据就将相应位置1 INV_WXYZ_QUAT标志就是读出的数据中存在四元数数据
    if(sensors&INV_WXYZ_QUAT)
    {
        q0 = quat[0] / q30;	//q30格式转换为浮点数 就是除以2^30
        q1 = quat[1] / q30;
        q2 = quat[2] / q30;
        q3 = quat[3] / q30;
        //计算得到俯仰角/横滚角/航向角
        *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	// pitch  *57.3是因为公式得到的是弧度而我们要角度
        *roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
        *yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw
    }else return 2;
    dmp_gyro_data[0] = gyro[0];
    dmp_gyro_data[1] = gyro[1];
    dmp_gyro_data[2] = gyro[2];
    return 0;
}

/**
 * @brief 自定义函数，通过dmp获取的角度值取微分得到角速度
 * @param angle：传入角度
 * @param angvel：接收角速度值
 * */

void MPU_DMP_Get_AngularVelocity(MPU_Data* mpu_data)
{
    mpu_data->angle_last = mpu_data->angle;
    mpu_data->angle = mpu_data->gyro_low_out;
    mpu_data->angvel = (mpu_data->angle - mpu_data->angle_last);
}






